﻿// EltControl.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include <elt_robot.h>
#include <cmath>
#include "MyTcp.h"
#include "Touchx.h"
#include "Pose2Matrix.h"
#include <Eigen/Eigen>
using namespace Eigen;
using namespace std;

static ELT_CTX ctx;
static elt_error err;
static int ret;
static elt_robot_pos pos_array;  //八轴关节角，最后两轴为0
static elt_robot_pose pose_array = { 0, 0, 0, -3.14, 0, -3.14 / 2 }; //位姿：X Y Z(单位毫米) RX RY RZ（单位弧度）
static elt_robot_pose target_point = { -579.562, -141.187, 335.192, -3.14, 0, -3.14 / 2 }; //位姿：X Y Z(单位毫米) RX RY RZ（单位弧度）
static elt_robot_pose initial_point = { -575.18, -157.666, 490.608, -3.14, 0, -3.14 / 2 }; //位姿：X Y Z(单位毫米) RX RY RZ（单位弧度）
elt_robot_pose current_pose;
static elt_robot_pos current_pos;
static elt_robot_torques tor_array;

// Params
double speed = 4; //移动速度
int state;
int robot_state;

int m = 10;
int k = 50;
double elt_ddz = 0;
double elt_dz = 0;
double elt_z = 0;

double elt_ddy = 0;
double elt_dy = 0;
double elt_y = 0;

double elt_ddx = 0;
double elt_dx = 0;
double elt_x = 0;
double* FTdata;
double v[3] = { 0, 0, 0 };

double rx, ry, rz;
double prev_rx, prev_ry, prev_rz;

Matrix3d R_TT_TB; //Rotation matrix of touchx tip coordinate in touchx base coordinate
Matrix3d R_TB_RB; //Rotation matrix of touchx base coordinate in robot base coordinate
Matrix3d R_TT_RB; //Rotation matrix of touchx tip coordinate in robot base coordinate
Matrix3d R_RE_RB; //Rotation matrix of robot end-effector coordinate in robot base coordinate
Matrix3d R_RE_TT; //Rotation matrix of robot end-effector coordinate in touchx tip coordinate
Matrix3d INV_R_TT_TB; 
Matrix3d INV_R_TB_RB; 
extern MatrixXd pose2matrix;

double* getDS(elt_robot_pose target_point, elt_robot_pose current_pose)
{
	double v[3] = {0, 0, 0};
	v[0] = -3 * (current_pose[0] - target_point[0])/100;
	v[1] = -3 * (current_pose[1] - target_point[1])/100;
	v[2] = -3 * (current_pose[2] - target_point[2])/100;
	return v;
}

int main()
{
	/****** touchx *********************************************************************************************/
	HDSchedulerHandle hUpdateHandle = 0;
	HDErrorInfo error;

	/* Initialize the device, must be done before attempting to call any hd
	   functions. */
	HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
	if (HD_DEVICE_ERROR(error = hdGetError()))
	{
		hduPrintError(stderr, &error, "Failed to initialize the device");
		fprintf(stderr, "\nPress any key to quit.\n");
		getch();
		return -1;
	}

	/* Schedule the main scheduler callback that updates the device state. */
	hUpdateHandle = hdScheduleAsynchronous(updateDeviceCallback, 0, HD_MAX_SCHEDULER_PRIORITY);

	/* Start the servo loop scheduler. */
	hdStartScheduler();
	if (HD_DEVICE_ERROR(error = hdGetError()))
	{
		hduPrintError(stderr, &error, "Failed to start the scheduler");
		fprintf(stderr, "\nPress any key to quit.\n");
		getch();
		return -1;
	}
	static const int kTerminateCount = 1000;
	int buttonHoldCount = 0;

	/* Instantiate the structure used to capture data from the device. */
	DeviceData currentData;
	DeviceData prevData;

	/* Perform a synchronous call to copy the most current device state. */
	for (int i = 0; i < 5; i++)
	{
		hdScheduleSynchronous(copyDeviceDataCallback, &currentData, HD_MIN_SCHEDULER_PRIORITY);
		memcpy(&prevData, &currentData, sizeof(DeviceData));
		fprintf(stdout, "%g, %g\n",
			currentData.m_devicePosition[0],
			prevData.m_devicePosition[0]);
	}

	//initial
	//matrix value
	R_RE_TT << 0, -1, 0,
		-1, 0, 0,
		0, 0, -1;
	R_TB_RB << 0, 0, -1,
		-1, 0, 0,
		0, 1, 0;
	INV_R_TB_RB = R_TB_RB.inverse();
	R_TT_TB << currentData.m_transform[0], currentData.m_transform[4], currentData.m_transform[8],
		currentData.m_transform[1], currentData.m_transform[5], currentData.m_transform[9],
		currentData.m_transform[2], currentData.m_transform[6], currentData.m_transform[10];
	R_RE_RB = R_TB_RB * R_TT_TB * R_RE_TT;
	Vector3d euler_angles = R_RE_RB.eulerAngles(2, 1, 0);       //按照ZYX顺序
	//cout << " yaw pitch row = " << euler_angles.transpose() << endl;
	rz = euler_angles[0];
	ry = euler_angles[1];
	rx = euler_angles[2];
	prev_rx = rx;
	prev_ry = ry;
	prev_rz = rz;

	printHelp();
	/***************************************************************************************************************/


	//创建连接
	ctx = elt_create_ctx("192.168.1.200", 8055);
	if (NULL == ctx)
		cout << "elt_create_ctx error\n";
	//登录
	ret = elt_login(ctx);
	if (ELT_SUCCESS == ret)
		cout << "elt_login success\n";
	else
	{
		cout << "elt_loing failed.\n";
		return -1;
	}


	//joint move
	/*ret = elt_inverse_kinematic(ctx, initial_point, pos_array, &err);
	ret = elt_joint_move(ctx, pos_array, speed, &err);
	elt_get_robot_state(ctx, &robot_state, &err);
	while (robot_state!=0)
	{
		ret = elt_get_robot_state(ctx, &robot_state, &err);
		cout << "robot_state is : " << robot_state << endl;
	}*/
	elt_get_robot_pose_common(ctx, current_pose, &err);
	pose_array[0] = current_pose[0]; 
	pose_array[1] = current_pose[1];
	pose_array[2] = current_pose[2];
	pose_array[3] = current_pose[3];
	pose_array[4] = current_pose[4];
	pose_array[5] = current_pose[5];


	//transparent transport initial
	ret = elt_get_transparent_transmission_state(ctx, &state, &err);
	while (state != 0)
	{
		ret=elt_transparent_transmission_clear_servo_joint_buffer(ctx, &err);
		ret = elt_get_transparent_transmission_state(ctx, &state, &err);
		cout << "state" << state << endl;
	}
	cout << "state" << state << endl;
	ret = elt_transparent_transmission_init(ctx, 400, 10, 1, &err);
	cout << "init ret " << ret << endl;
	ret = elt_get_transparent_transmission_state(ctx, &state, &err);
	elt_get_robot_pos(ctx, pos_array, &err);
	ret = elt_transparent_transmission_put_servo_joint_to_buffer(ctx, pos_array, &err);

	//主控制程序while循环
	//MyTcp myTCP;
	while(state==1)
	{
		
		/**********************************************************************/
		/*读取touchx坐标，向右为x轴正方向，向上为y轴正方向，向前为z轴正方向 */
		hdScheduleSynchronous(copyDeviceDataCallback, &currentData, HD_MIN_SCHEDULER_PRIORITY);
		/*********************************************************************/

		//读取力矩数据
		/*FTdata = myTCP.RecvData();
		if (myTCP.flag == -1)
		{
			break;
		}*/

		//force control
		/*elt_get_robot_torques(ctx, tor_array, &err);
		cout << "力矩：" << tor_array[0] << " " << tor_array[1] << " " << tor_array[2] << " "
			<< tor_array[3] << " " << tor_array[4] << " " << tor_array[5] << " " << endl;*/

		//获取当前末端位置并计算下一时刻位置
		/*elt_get_robot_pose_common(ctx, current_pose, &err);
		pose_array[0] = current_pose[0] + getDS(target_point, current_pose)[0];
		pose_array[1] = current_pose[1] + getDS(target_point, current_pose)[1];
		pose_array[2] = current_pose[2] + getDS(target_point, current_pose)[2];*/

		
		
		//transform
		R_TT_TB << currentData.m_transform[0], currentData.m_transform[4], currentData.m_transform[8],
			currentData.m_transform[1], currentData.m_transform[5], currentData.m_transform[9],
			currentData.m_transform[2], currentData.m_transform[6], currentData.m_transform[10];
		INV_R_TT_TB = R_TT_TB.inverse();
		 
		/*ry = atan2(-currentData.m_transform[2], sqrt(pow(currentData.m_transform[0], 2.0) + pow(currentData.m_transform[1], 2.0)));
		rz = atan2(currentData.m_transform[1] / cos(ry), currentData.m_transform[0] / cos(ry));
		rx = atan2(currentData.m_transform[6] / cos(ry), currentData.m_transform[10] / cos(ry));*/

		elt_get_robot_pose_common(ctx, current_pose, &err);

		if (currentData.m_buttonState && !prevData.m_buttonState)
		{
			//get robot end-effector rotation matrix
			Pose2Matrix(current_pose);
			R_RE_RB = pose2matrix.block(0, 0, 3, 3);
			R_RE_TT = INV_R_TT_TB * INV_R_TB_RB * R_RE_RB;
		}

		R_RE_RB = R_TB_RB * R_TT_TB * R_RE_TT;
		Vector3d euler_angles = R_RE_RB.eulerAngles(2, 1, 0);       //按照ZYX顺序		
		rz = euler_angles[0];
		ry = euler_angles[1];
		rx = euler_angles[2];

		if (currentData.m_buttonState && prevData.m_buttonState)//press button
		{
			cout << "press" << endl;
			cout << "yaw pitch row = " << euler_angles.transpose() << endl;

			/***incorrect version***/
			//pose_array[0] = current_pose[0] - (currentData.m_devicePosition[2] - prevData.m_devicePosition[2]);  //倍数注意别写错地方！！！
			//pose_array[1] = current_pose[1] - (currentData.m_devicePosition[0] - prevData.m_devicePosition[0]);
			//pose_array[2] = current_pose[2] + (currentData.m_devicePosition[1] - prevData.m_devicePosition[1]);
			//pose_array[3] = current_pose[3] - (currentData.m_gimbalAngles[1] - prevData.m_gimbalAngles[1]);
			//pose_array[4] = current_pose[4] + (currentData.m_gimbalAngles[0] - prevData.m_gimbalAngles[0]);
			//pose_array[5] = current_pose[5] + (currentData.m_gimbalAngles[2] - prevData.m_gimbalAngles[2]);

			/*cout << "current_pose：" << current_pose[0] << " " << current_pose[1] << " " << current_pose[2] << " "
				<< current_pose[3] << " " << current_pose[4] << " " << current_pose[5] << " " << endl;
			cout << "next_pose：" << pose_array[0] << " " << pose_array[1] << " " << pose_array[2] << " "
				<< pose_array[3] << " " << pose_array[4] << " " << pose_array[5] << " " << endl;
			cout << endl;*/

			pose_array[0] = current_pose[0] - (currentData.m_devicePosition[2] - prevData.m_devicePosition[2])*2;  //倍数注意别写错地方！！！
			pose_array[1] = current_pose[1] - (currentData.m_devicePosition[0] - prevData.m_devicePosition[0])*2;
			pose_array[2] = current_pose[2] + (currentData.m_devicePosition[1] - prevData.m_devicePosition[1])*2;
			/*pose_array[3] = current_pose[3] + (rx - prev_rx);
			pose_array[4] = current_pose[4] + (ry - prev_ry);
			pose_array[5] = current_pose[5] + (rz - prev_rz);*/
			pose_array[3] = rx;
			pose_array[4] = ry;
			pose_array[5] = rz;
		}
		

		////x-axis
		//elt_ddx = (FTdata[0]*1000 - k * elt_dx) / m;
		//elt_dx = elt_dx + elt_ddx * 0.2;
		//pose_array[2] = pose_array[2] + elt_dx * 0.01;

		////z-axis
		//elt_ddz = (FTdata[2]*1000 - k * elt_dz) / m;
		//elt_dz = elt_dz + elt_ddz * 0.2;
		//pose_array[2] = pose_array[2] - elt_dz*0.01 ;

		//透传
		elt_get_robot_pos(ctx, current_pos, &err);
		ret = elt_inverse_kinematic_ref(ctx, pose_array, current_pos, pos_array, &err);
			
		/*cout << "current_pos：" << current_pos[0] << " " << current_pos[1] << " " << current_pos[2] << " "
			<< current_pos[3] << " " << current_pos[4] << " " << current_pos[5] << " " << endl;
		cout << "next_pos：" << pos_array[0] << " " << pos_array[1] << " " << pos_array[2] << " "
			<< pos_array[3] << " " << pos_array[4] << " " << pos_array[5] << " " << endl;
		cout << endl;*/

		ret = elt_transparent_transmission_put_servo_joint_to_buffer(ctx, pos_array, &err);
		Sleep(10);

		/***********************************************************/
		/* Store off the current data for the next loop. */
		prev_rx = rx;
		prev_ry = ry;
		prev_rz = rz;
		memcpy(&prevData, &currentData, sizeof(DeviceData));
		/***********************************************************/
	}


	//myTCP.~MyTcp();
	ret = elt_transparent_transmission_clear_servo_joint_buffer(ctx, &err);
	ret = elt_get_transparent_transmission_state(ctx, &state, &err);
	while (state != 0)
	{
		ret = elt_transparent_transmission_clear_servo_joint_buffer(ctx, &err);
		ret = elt_get_transparent_transmission_state(ctx, &state, &err);
		cout << "state" << state << endl;
	}
	cout << "state" << state << endl;

	//退出
	ret = elt_logout(ctx);
	if (ELT_SUCCESS == ret)
		cout << "elt_logout success.\n";
	else
	{
		cout << "elt_logout failed.\n";
		return -1;
	}
	//销毁链接
	ret = elt_destroy_ctx(ctx);
	if (ELT_SUCCESS == ret)
		cout << "elt_destroy_ctx success.\n";
	else
	{
		cout << "elt_destroy_ctx failed.\n";
		return -1;
	}


	/* For cleanup, unschedule callbacks and stop the servo loop. */
	hdStopScheduler();
	hdUnschedule(hUpdateHandle);
	hdDisableDevice(hHD);
	return 0;

}
